#include "ping.h"
#include "err.h"
#include "sockets.h"
#include "os_io.h"
#include "os_mem.h"
#include "process.h"
#include "user_timers.h"
#include "i2c.h"


static void toggle_led(void *arg)
{
    static bool state = false;
    bool switch_state = false;
    err_t err_code;

    // Read DIP switch 1
    //printf("Reading DIP switch value\n");

    err_code = read_dip_switch(0, &switch_state);
    if (err_code != ERR_SUCCESS) {
        printf("Failed to read DIP switch status. err:0x%x\n", err_code);
	return;
    }
    //printf("DIP switch value is: 0x%x\n", switch_state);

    if (!switch_state) {     // Read first DIP switch
        // DIP switch is OFF.
	if (state) {
	    // LED is glowing, switch it off.
	    glow_led(LED1, false);
	    state = false;
	}

	return;
    }

    // DIP switch is ON
    // Start blinking the LED1
    if (!state) {
        // OFF => ON 
	glow_led(LED1, true);
    } else {
        // ON => OFF
	glow_led(LED1, false);
    }

    state = !state;

    return;
}

void start_ping_internal(void)
{
    void *sock = NULL;
    err_t err_code;
    socket_data_t sock_data;
    icmp_t *icmp_msg = NULL;
    icmp_t icmp_req;
    user_timer_data_t *led_timer = NULL;
    uint16_t len = 0;
    uint16_t i = 0;

    err_code = open_ip_socket((uint8_t *)"icmp_socket", 1, &sock);  // icmp is IP protocol 1
    if (err_code != ERR_SUCCESS) {
        printf("Could not open ip socket for port 1. err:0x%x\n", err_code);
	return;
    }



    led_timer = (user_timer_data_t *)calloc(1, sizeof(user_timer_data_t));
    if (!led_timer) {
        printf("Failed to allocate timer for led blink\n");
    } else {
        led_timer->interval = 500; //500ms
	led_timer->periodic = true;
	led_timer->user_handler = toggle_led;
	led_timer->user_handler_arg = NULL;
	err_code = socket_timer_start(sock, led_timer);
	if (err_code != ERR_SUCCESS) {
	    printf("Failed to start led timer\n");
	    free(led_timer);
	}
    }

    while (1) { 
        len = sizeof(icmp_req);
        err_code = rcv_socket(sock, &sock_data, (uint8_t *)&icmp_req, &len);
        if (err_code != ERR_SUCCESS) {
            printf("Unable to receive ICMP on socket: 0x%x. Exiting.\n", err_code);
            socket_close(sock);
	    return;
        }

	if (icmp_req.type != ECHO_REQ) {
	    printf("ICMP message other than ECHO REQUEST received. Not handled\n");
	    continue;
	}

        icmp_msg = (icmp_t *)calloc(1, len);
        if (!icmp_msg) {
            printf("Could not allocate icmp message\n");
	    continue;
        }

        icmp_msg->type = ECHO_REPLY;
        icmp_msg->code = icmp_req.code;
        icmp_msg->checksum = 0;
        icmp_msg->quench.value = icmp_req.quench.value;

    
        for (i = 0; i < (len - OFFSET_OF(data, icmp_t)); i++) {
            icmp_msg->data[i] = icmp_req.data[i];
        }
	//printf("Copied data of length: 0x%x\n", i);

	icmp_msg->checksum = calc_ip_checksum((uint16_t *)icmp_msg, (len/2));
    
        sock_data.data = (uint8_t *)icmp_msg;
	sock_data.len = len;
	//printf("Sending ICMP response\n");
        err_code = send_on_socket(sock, &sock_data);
        if (err_code != ERR_SUCCESS) {
            printf("ICMP response send on socket failed!!\n");
	    continue;
        } 
    }
    
    return;
}

void start_icmp(void)
{
   exec_mem(start_ping_internal, (const uint8_t *)"ping", 0, PROCESS_USER_PRIORITY); 
   return;
}


